/* Private includes ----------------------------------------------------------*/
#include "fliter.h"
/* Private define ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
IIR_BUTTERWORTH_DEF Rpm_IIR_LPF;
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/

void IIR_Butterworth_Coefficient_Init(float temp[8], IIR_BUTTERWORTH_DEF* iir_butterworth_temp)
{
  iir_butterworth_temp->b0      = temp[0];
  iir_butterworth_temp->b1      = temp[1];
  iir_butterworth_temp->b2      = temp[2];
  iir_butterworth_temp->a0      = temp[3];
  iir_butterworth_temp->a1      = temp[4];
  iir_butterworth_temp->a2      = temp[5];
  iir_butterworth_temp->gain0   = temp[6];
  iir_butterworth_temp->gain1   = temp[7];
  iir_butterworth_temp->states0 = 0.0f;
  iir_butterworth_temp->states1 = 0.0f;
}

void IIR_Butterworth(float in, float* out, IIR_BUTTERWORTH_DEF* iir_butterworth_temp)
{
  float temp;
  temp = (iir_butterworth_temp->gain0 * in - iir_butterworth_temp->a1 * iir_butterworth_temp->states0) - iir_butterworth_temp->a2 * iir_butterworth_temp->states1;
  *out = ((iir_butterworth_temp->b0 * temp + iir_butterworth_temp->b1 * iir_butterworth_temp->states0) + iir_butterworth_temp->b2 * iir_butterworth_temp->states1) * iir_butterworth_temp->gain1;

  iir_butterworth_temp->states1 = iir_butterworth_temp->states0;
  iir_butterworth_temp->states0 = temp;
}

void IIR_Butterworth_Init()
{
  // float Rpm_IIR_Param[8] = {1.0f, 2.0f, 1.0f, 1.0f, -1.56101807580071816f, 0.641351538057563175f, 0.020083365564211235f, 1.0f};
  float Rpm_IIR_Param[8] = {1.0f, 2.0f, 1.0f, 1.0f, -1.77863177782458481424f, 0.80080264666570755f, 0.005542717210280681f, 1.0f};
  IIR_Butterworth_Coefficient_Init(Rpm_IIR_Param, &Rpm_IIR_LPF);
}
